#!/usr/bin/env python3
import yaml

output_path="node_graph.yaml"

nodes=[
    {
        "block_name" : "floor7",
        "launch_file" : "$(find navigation_pkg)/multi_mode_launch/floor7.launch",
        "map_file" : "$(find navigation_pkg)/multi_mode_configure/floor7.yaml",
        "pcd_file_path" : "$(find navigation_pkg)/multi_mode_configure/floor7.pcd",
        "default_pose" : [0,0,0,0,0,0] # x y z roll pitch yaw
    },
    {
        "block_name" : "floor9",
        "launch_file" : "$(find navigation_pkg)/multi_mode_launch/floor9.launch",
        "map_file" : "$(find navigation_pkg)/multi_mode_configure/floor9.yaml",
        "pcd_file_path" : "$(find navigation_pkg)/multi_mode_configure/floor9.pcd",
        "default_pose" :[0,0,0,0,0,0],
    },
    {
        "block_name" : "stair",
        "launch_file" : "$(find navigation_pkg)/multi_mode_launch/stair.launch",
        "map_file" :  "$(find navigation_pkg)/multi_mode_configure/stair1.yaml",
        "pcd_file_path" : "$(find navigation_pkg)/multi_mode_configure/stair1.pcd",
        "default_pose" : [0,0,0,0,0,0] # x y z roll pitch yaw
    }
]

edges=[
    {
        "from" : "floor7",
        "to" : "stair",
        "switch_point_at_from" : [6.75,-7.5,-0.17,0,0,-1.63], # x y z roll pitch yaw
        "switch_point_at_to" : [0,0,0,0,0,0]
    },
    {
        "from" : "stair",
        "to" : "floor9",
        "switch_point_at_from" : [0.3,0.05,6.99,0.0,0.0,-1.75], # x y z roll pitch yaw
        "switch_point_at_to" : [0.1045358812684411, -7.497576700681314, 0.176346845460426, 6.25019697646124, 0.01106413737631076, 6.272498635150232-3.1415926]
    },
    {
        "from" : "floor9",
        "to" : "stair",
        "switch_point_at_from" : [0.1045358812684411, -7.497576700681314, 0.176346845460426, 6.25019697646124, 0.01106413737631076, 6.272498635150232],
        "switch_point_at_to" : [0.20,0.27,7.02,0,0,3.1415926-1.6481527709571642]
    },
    {
        "from" : "stair",
        "to" : "floor7",
        "switch_point_at_from" : [0,0,0,0,0,-3.14], # x y z roll pitch yaw
        "switch_point_at_to" : [6.75,-7.5,-0.17,0,0,1.63]
    },
]

graph = {
    'nodes': nodes,
    'edges': edges
}

with open(output_path, 'w') as f:
    yaml.dump(graph, f, default_flow_style=False)

print(">>>>>>>>>>>> node graph created <<<<<<<<<<<<<")